#include <Arduino.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "config.h"
#include "ADC.h"

#define ADC_VREF_TYPE 0xC0

unsigned int adc_data[LAST_ADC_INPUT-FIRST_ADC_INPUT+1];
unsigned int adc_filtered_data[LAST_ADC_INPUT-FIRST_ADC_INPUT+1];


// this function work only for robot speebo!!! 
// окно медианного фильтра 3 всего масив состоит из 5 элементов
#define ARRAY_FOR_MEDIAN 3
static unsigned int vector_fil[LAST_ADC_INPUT-FIRST_ADC_INPUT+1][ARRAY_FOR_MEDIAN];
static int i =0;
static int j =0;

void medianFilter(int indexSensor)
{
  //vector_fil[indexSensor][0] = vector_fil[indexSensor][1];
  //vector_fil[indexSensor][1] = vector_fil[indexSensor][2];
  //vector_fil[indexSensor][2] = adc_data[indexSensor];
  vector_fil[indexSensor][i=(++i)%ARRAY_FOR_MEDIAN] = adc_data[indexSensor];
  // sort
  for (int i0 = 0; i0<ARRAY_FOR_MEDIAN;i0++)
  {
    for(int i1 = 0; i1<ARRAY_FOR_MEDIAN-1;i1++)
    {
      if(vector_fil[indexSensor][i1]>vector_fil[indexSensor][i1+1])
      {
        unsigned int varTemp = vector_fil[indexSensor][i1+1];
        vector_fil[indexSensor][i1+1] = vector_fil[indexSensor][i1];
        vector_fil[indexSensor][i1] = varTemp;
      }
    }
  }
  adc_filtered_data[indexSensor] = vector_fil[indexSensor][ARRAY_FOR_MEDIAN/2];
  
}

 
 
 
 
 

static unsigned char input_index=0;

// ADC interrupt service routine
// with auto input scanning
ISR (ADC_vect)
{
    // Read the AD conversion result
    adc_data[input_index]=ADCW;
    // Select next ADC input
    if (++input_index > (LAST_ADC_INPUT-FIRST_ADC_INPUT))
       input_index=0;
    ADMUX=(ADC_VREF_TYPE & 0xff) | ((FIRST_ADC_INPUT+input_index) & 0x07);
    if ((FIRST_ADC_INPUT+input_index) & 0x08) ADCSRB |= 0x10;
    else ADCSRB &= 0xef;
    // Delay needed for the stabilization of the ADC input voltage
    medianFilter(input_index);
    delayMicroseconds(10);
    // Start the AD conversion
    ADCSRA|=0x40;
    
}

void adcInit()
{
  //mega128
  // ADC initialization
  // ADC Clock frequency: 250,000 kHz
  // ADC Voltage Reference: 2.56V, cap. on AREF
  // ADC Auto Trigger Source: None
  // Digital input buffers on ADC0: On, ADC1: On, ADC2: On, ADC3: On
  // ADC4: On, ADC5: On, ADC6: On, ADC7: On
  DIDR0=0x00;
  // Digital input buffers on ADC8: On, ADC9: On, ADC10: On, ADC11: On
  // ADC12: On, ADC13: On, ADC14: On, ADC15: On
  DIDR2=0x00;
  ADMUX=(FIRST_ADC_INPUT & 0x07) | (ADC_VREF_TYPE & 0xff);
  ADCSRA=0xCE;
  ADCSRB |=FIRST_ADC_INPUT & 0x08;
}


